#include "AIEngine.h"
#include "Game.h"

AIEngine::AIEngine(Camera *camera_, KdTree *kdTree_) { 
	camera = camera_; 
	enemies = new std::vector<Enemy*>();
	kdTree = kdTree_;
}

void AIEngine::placeEnemy(unsigned int index) {
	if (index < enemies->size()) {
		Enemy* enemy = enemies->at(index);
		glTranslatef(enemy->x, enemy->y, enemy->z);
		glRotatef(270, 0, 1, 0);	
		glRotatef(enemy->rotateX, 0, 1, 0);	
		glScalef(10.0f, 10.0f, 10.0f);
	}
}

void AIEngine::placeCharacter() {
	glTranslatef(camera->x, 0, camera->z);
	glRotatef(90, 0, 1, 0);			
	glRotatef(camera->rotateX, 0, 1, 0);
	glScalef(10.0f, 10.0f, 10.0f);
}

void AIEngine::updateEnemies() {
	for (unsigned int i=0; i < enemies->size(); i++) {
		Enemy *enemy = enemies->at(i);
		if (!enemy->following) {
			updateAutonomousEnemy(enemy);
		}		
	}
	for (unsigned int i=0; i < camera->getFollowersSize(); i++) {
		if (i == 0) {
			Enemy *enemyFollower = new Enemy(camera->x, camera->y, camera->z, 0, kdTree);
			updateFollowingEnemy(camera->getFollower(i), enemyFollower);
		} else {
			updateFollowingEnemy(camera->getFollower(i), camera->getFollower(i - 1));	
		}
	}
}

bool AIEngine::isFrontCollides(Enemy *enemy, int ratio) {
	bool frontCollides = false;
	//Does front collide?
  	aiVector3D oldEnemyPos(enemy->x, enemy->y, enemy->z);	
	enemy->move(ratio);
  	aiVector3D newEnemyPos(enemy->x, enemy->y, enemy->z);
	aiVector3D dir = newEnemyPos - oldEnemyPos;
        
	if (kdTree->cameraCollisionTest(oldEnemyPos, dir)){
		frontCollides = true;
	}
	enemy->move(-ratio);

	return frontCollides;
}

bool AIEngine::isLeftCollides(Enemy *enemy, int ratio) {
	bool leftCollides = false;
	//Does the left collide?
  	aiVector3D oldEnemyPos(enemy->x, enemy->y, enemy->z);	
	enemy->move(ratio/2);	
	enemy->strafe(-ratio * 2.1);
  	aiVector3D newEnemyPos(enemy->x, enemy->y, enemy->z);
	aiVector3D dir = newEnemyPos - oldEnemyPos;

        if (kdTree->cameraCollisionTest(oldEnemyPos, dir)) {
		leftCollides = true;
	}
	enemy->strafe(ratio * 2.1);
	enemy->move(-ratio/2);
	return leftCollides;
}

bool AIEngine::isRightCollides(Enemy *enemy, int ratio) {
	bool rightCollides = false;
	//Does the right collide?
  	aiVector3D oldEnemyPos(enemy->x, enemy->y, enemy->z);	
	enemy->move(ratio/2);	
	enemy->strafe(ratio * 2.1);
  	aiVector3D newEnemyPos(enemy->x, enemy->y, enemy->z);
	aiVector3D dir = newEnemyPos - oldEnemyPos;
        if (kdTree->cameraCollisionTest(oldEnemyPos, dir)) {
		rightCollides = true;
	}
	enemy->strafe(-(ratio * 2.1));							
	enemy->move(-ratio/2);
	return rightCollides;
}

void AIEngine::updateAutonomousEnemy(Enemy *enemy) {

	int ratio = 14;
	bool frontCollides = isFrontCollides(enemy, ratio);
	bool rightCollides = isRightCollides(enemy, ratio);
	bool leftCollides = isLeftCollides(enemy, ratio);

	//printf("###########frontCollides=%d		leftCollides=%d		rightCollides=%d\n", frontCollides, leftCollides, rightCollides);

	if (!frontCollides) {
		//printf("1\n", leftCollides, rightCollides);
		enemy->move(enemy->speed);
	} else if (frontCollides && rightCollides && leftCollides) {
		//printf("2\n");
		enemy->rotateXDegrees(180);
		enemy->move(-enemy->speed);				
	} else if (frontCollides && !rightCollides && leftCollides) {
		//printf("3\n");
		enemy->rotateXDegrees(-90);
		enemy->move(enemy->speed);
	} else if (frontCollides && rightCollides && !leftCollides) {
		//printf("4\n");
		enemy->rotateXDegrees(90);
		enemy->move(enemy->speed);

	} else if (frontCollides && !rightCollides && !leftCollides) {
		//printf("5\n");
		if (rand() % 2 == 1) {
			enemy->rotateXDegrees(90);
			enemy->move(enemy->speed);			
		} else {
			enemy->rotateXDegrees(-90);
			enemy->move(enemy->speed);
		}
		//enemy->move(enemy->speed);

	} else if (!frontCollides && (!leftCollides || !rightCollides)) {
		//printf("6\n");
		if (!leftCollides) {
			enemy->rotateXDegrees(90);
			enemy->move(enemy->speed);			
		} else {
			enemy->rotateXDegrees(-90);
			enemy->move(enemy->speed);
		}
	} else if (!frontCollides && !rightCollides && !leftCollides) {
		printf("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$7\n");
		//enemy->move(enemy->speed);

	}
}

void AIEngine::updateFollowingEnemy(Enemy *enemy, Enemy *follow) {
	aiVector3D followPos(follow->x, 0.3, follow->z);
	aiVector3D enemyPos(enemy->x, 0.3, enemy->z);
	float charToEnemyLength = (followPos - enemyPos).Length();
	float newCharToEnemyLength = 0;

	int ratio = 14;
	bool frontCollides = isFrontCollides(enemy, ratio);
	bool rightCollides = isRightCollides(enemy, ratio);
	bool leftCollides = isLeftCollides(enemy, ratio);
	
	if (charToEnemyLength < 15) {
		return;	
	}	

	//Move Forward?
	if (!frontCollides) {
		enemy->move(enemy->speed);
		aiVector3D newEnemyPos(enemy->x, 0.3, enemy->z);
	 	newCharToEnemyLength = (followPos - newEnemyPos).Length();
		if (charToEnemyLength > newCharToEnemyLength) {
			return;
		} else {
			enemy->x = enemyPos.x; enemy->z = enemyPos.z;		
		}
	}
	if (!leftCollides) {
		enemy->rotateXDegrees(90);
		enemy->move(enemy->speed);
		aiVector3D newEnemyPos(enemy->x, 0.3, enemy->z);
	 	newCharToEnemyLength = (followPos - newEnemyPos).Length();
		if (charToEnemyLength > newCharToEnemyLength) {
			return;
		} else {
			enemy->x = enemyPos.x; enemy->z = enemyPos.z;
			enemy->rotateXDegrees(-90);		
		}
	}
	if (!rightCollides) {
		enemy->rotateXDegrees(-90);
		enemy->move(enemy->speed);
		aiVector3D newEnemyPos(enemy->x, 0.3, enemy->z);
	 	newCharToEnemyLength = (followPos - newEnemyPos).Length();
		if (charToEnemyLength > newCharToEnemyLength) {
			return;
		} else {
			enemy->x = enemyPos.x; enemy->z = enemyPos.z;
			enemy->rotateXDegrees(90);		
		}
	}
	
/*
	printf("Stuck move either forward or backward");
	if (leftCollides && enemy->x - 10 > follow->x) {
		enemy->move(enemy->speed);
	} else if (leftCollides && enemy->x + 10 < follow->x) {
		enemy->move(-enemy->speed);	
	}
*/
	enemy->rotateXDegrees(180);
	enemy->move(-enemy->speed);
	aiVector3D newEnemyPos(enemy->x, 0.3, enemy->z);
	newCharToEnemyLength = (followPos - newEnemyPos).Length();


}

void AIEngine::characterCollidesWithEnemy() {
	for (unsigned int i=0; i < enemies->size(); i++) {
		Enemy *enemy = enemies->at(i);
		if (enemy->following) {
			continue;		
		}
		if (enemy->x < camera->x + 5 && enemy->x > camera->x - 5 && 
			enemy->z < camera->z + 5 && enemy->z > camera->z -5) {
			enemy->following = true;
			enemy->speed = 1.5;
			camera->addFollower(enemy);	
		} 

	}

}

void AIEngine::addEnemy(GLdouble x, GLdouble y, GLdouble z, GLdouble speed) {
	Enemy *enemy = new Enemy(x, y, z, speed, kdTree);
	enemies->push_back(enemy);
}
